import rclpy
import json
import os
import time
import numpy as np
import threading
from collections import deque
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from lane_detection.LaneDetector import LaneDetector
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Point
from detection_interfaces.msg import DetectionDataLaneline, DetectionStateLaneline
from lane_detection.read_param import LaneDetectionConfig
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy

class LaneDetectionNode(Node):
    """车道线检测节点。
    
    该节点负责接收原始图像，进行车道线检测，并发布检测结果。
    
    Attributes:
        config (LaneDetectionConfig): 车道线检测配置参数。
        lane_detector (LaneDetector): 车道线检测器实例。
        bridge (CvBridge): OpenCV与ROS图像转换桥。
        image_publisher (Publisher): 检测结果图像发布器。
        center_points_publisher (Publisher): 车道中心点发布器。
        laneline_data_publisher (Publisher): 车道线数据发布器。
        laneline_state_publisher (Publisher): 车道线状态发布器。
        subscription (Subscription): 图像订阅器。
        state_timer (Timer): 状态发布定时器。
        data_queue (Queue): 数据共享队列。
        publish_thread (Thread): 数据发布线程。
        node_state (int): 节点状态 (0:正常, 1:异常)。
        error_code (int): 错误代码。
    """
    
    def __init__(self):
        """初始化车道线检测节点。
        
        加载配置参数，初始化车道线检测器，创建发布者和订阅者。
        """
        super().__init__('lane_detection')
        
        # 从YAML文件加载配置
        self.config = LaneDetectionConfig.from_yaml('src/lane_detection/config.yaml')
        
        # 获取参数
        input_topic = '/lane_detection/image_raw'
        output_image_topic = '/lane_detection/image'
        center_points_topic = '/lane_centers'

        # 初始化车道检测器
        self.lane_detector = LaneDetector(self.config)
        
        # 初始化CvBridge
        self.bridge = CvBridge()
        
        # 创建发布者
        if self.config.vis_config.output_frame == 'topic':
            self.image_publisher = self.create_publisher(Image, output_image_topic, 2)
        self.center_points_publisher = self.create_publisher(MarkerArray, center_points_topic, 2)
        
        # 创建标线检测数据发布者 (10Hz)
        # 使用高优先级QoS配置
        qos_profile = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE,
            durability=QoSDurabilityPolicy.VOLATILE,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10  # 队列深度
        )

        self.laneline_data_publisher = self.create_publisher(
            DetectionDataLaneline, 
            '/detection/data/laneline', 
            qos_profile=qos_profile
        )
        
        # 创建标线检测状态发布者 (2Hz)
        self.laneline_state_publisher = self.create_publisher(
            DetectionStateLaneline, 
            '/detection/state/laneline', 
            2
        )
        
        # 创建订阅者
        self.subscription = self.create_subscription(
            Image,
            input_topic,
            self.image_callback,
            1
        )
        
        # 创建定时器用于状态发布 (2Hz)
        self.state_timer = self.create_timer(0.5, self.publish_state)
        self.data_timer = self.create_timer(0.1, self.data_publisher)

        # 初始化状态变量
        self.node_state = 0  # 0:正常, 1:异常
        self.error_code = 0  # 错误码
        
        # 存储检测结果
        self.img_left_line = []
        self.img_right_line = []
        self.img_mid_line = []
        self.bev_left_line = []
        self.bev_right_line = []
        self.bev_mid_line = []
        
        # 创建数据共享队列 (最多缓存10个消息)
        self.latest_data = deque(maxlen=1)
        # 创建锁保护共享数据
        self.data_lock = threading.Lock()
        
        # 完成初始化
        self.get_logger().info(f'Lane Detection Node initialized')
        self.get_logger().info(f'Subscribing to: {input_topic}')
        if self.config.vis_config.output_frame == 'topic':
            self.get_logger().info(f'Publishing to: {output_image_topic}, {center_points_topic}, /detection/data/laneline, /detection/state/laneline')
        else:
            self.get_logger().info(f'Publishing to: {center_points_topic}, /detection/data/laneline, /detection/state/laneline')
        self.get_logger().info('Data publisher thread started (10Hz)')

    def load_config(self, config_path: str) -> dict:
        """从JSON文件加载配置。
        
        Args:
            config_path (str): JSON配置文件路径。
            
        Returns:
            dict: 加载的配置字典。
        """
        try:
            # 检查文件是否存在
            if not os.path.isfile(config_path):
                self.get_logger().warn(f"配置文件不存在: {config_path}")
                return {}
            
            # 读取并解析JSON文件
            with open(config_path, 'r') as f:
                config = json.load(f)
            
            self.get_logger().info(f"成功加载配置文件: {config_path}")
            return config
            
        except Exception as e:
            self.get_logger().error(f"加载配置文件失败: {str(e)}")
            return {}
        
    def get_visable_mark(self, center_points: list) -> Marker:
        """创建可视化的车道中心线标记。
        
        Args:
            center_points (list): 车道中心点列表。
            
        Returns:
            Marker: 包含车道中心线的可视化标记。
        """
        marker = Marker()
        marker.header.frame_id = "map"
        marker.header.stamp = self.get_clock().now().to_msg()
        marker.ns = "center_line"
        marker.id = 0
        marker.type = Marker.LINE_STRIP
        marker.action = Marker.ADD
        
        # 设置标记属性
        marker.scale.x = 0.1  # 线宽
        marker.color.r = 0.0
        marker.color.g = 1.0  # 绿色
        marker.color.b = 0.0
        marker.color.a = 1.0  # 不透明度
        
        # 添加点
        for point in center_points:
            p = Point()
            p.x = float(point[0])  # X坐标
            p.y = float(point[1])  # Y坐标
            p.z = float(point[2])  # Z坐标（通常为0）
            marker.points.append(p)
        
        return marker
    
    def data_publisher(self):
        """
        发布车道线检测数据 (10Hz)。
        """

        # 使用锁保护共享数据
        with self.data_lock:
            # 获取最新数据（如果有）
            if self.latest_data:
                data = self.latest_data[0]
            else:
                data = None
        
        # 发布数据
        if data is not None:
            self.laneline_data_publisher.publish(data)
            self.get_logger().info(f"发布最新数据，推理耗时: {data.process_time:.3f} 秒")
        else:
            # 没有数据，发布空数据保持频率
            empty_data = DetectionDataLaneline()
            empty_data.timestamp = time.time()
            self.laneline_data_publisher.publish(empty_data)
            self.get_logger().debug("没有数据，发布空数据")
            
    def publish_state(self):
        """发布车道线检测状态 (2Hz)。
        
        该方法定时发布节点的当前状态和错误码。
        """
        msg = DetectionStateLaneline()
        msg.timestamp = time.time()
        msg.state = self.node_state
        msg.error = self.error_code
        
        self.laneline_state_publisher.publish(msg)
    
    def image_callback(self, msg: Image):
        """图像回调函数。
        
        处理接收到的图像消息，进行车道线检测并存储结果。
        
        Args:
            msg (Image): 接收到的ROS图像消息。
        """
        try:
            # 转换ROS图像消息为OpenCV格式
            cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
            _, width = cv_image.shape[:2]
            frame_left = cv_image[:, :width//2, :]  # 左半部分
            frame_right = cv_image[:, width//2:, :]  # 右半部分

            # 处理图像
            annotated_frame, return_param, center_points = self.lane_detector.process_img(frame_left, frame_right)
            
            # 发布检测结果图像
            if annotated_frame is not None and self.config.vis_config.output_frame == 'topic':
                image_msg = self.bridge.cv2_to_imgmsg(annotated_frame, encoding='bgr8')
                image_msg.header = msg.header
                self.image_publisher.publish(image_msg)
            
            # 发布车道中心点数据
            if center_points:
                center_marker = self.get_visable_mark(center_points)
                center_msg = MarkerArray()
                center_msg.markers.append(center_marker)
                self.center_points_publisher.publish(center_msg)
            
            # 创建检测数据消息
            if isinstance(return_param, dict):
                # 添加处理时间
                return_param['timestamp'] = time.time()
                # 确保所有字段存在
                for key in ['img_number', 'img_left_line', 'img_right_line', 'img_mid_line',
                           'bev_number', 'bev_left_line', 'bev_right_line', 'bev_mid_line']:
                    if key not in return_param:
                        return_param[key] = 0 if 'number' in key else []
                
                # 创建消息对象
                detection_data = DetectionDataLaneline()
                detection_data.timestamp = time.time()
                detection_data.process_time = return_param['process_time']
                detection_data.img_number = return_param['img_number']
                detection_data.img_left_line = return_param['img_left_line']
                detection_data.img_right_line = return_param['img_right_line']
                detection_data.img_mid_line = return_param['img_mid_line']
                detection_data.bev_number = return_param['bev_number']
                detection_data.bev_left_line = return_param['bev_left_line']
                detection_data.bev_right_line = return_param['bev_right_line']
                detection_data.bev_mid_line = return_param['bev_mid_line']
                
                # 使用锁保护共享数据
                with self.data_lock:
                    # 更新最新数据（自动丢弃旧数据）
                    self.latest_data.clear()
                    self.latest_data.append(detection_data)
            
            # 重置状态
            self.node_state = 0
            self.error_code = 0
            
        except Exception as e:
            self.get_logger().error(f'Error processing image: {str(e)}')
            self.node_state = 1  # 设置为异常状态
            self.error_code = 1  # 设置错误码

def main(args=None):
    """主函数。
    
    初始化ROS 2，创建节点并运行。
    
    Args:
        args: 命令行参数。
    """
    rclpy.init(args=args)
    node = LaneDetectionNode()
    
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        # 等待发布线程结束
        node.get_logger().info("等待数据发布线程结束...")
        node.publish_thread.join(timeout=1.0)
        
        # 清理资源
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()